//
// Created by gaoxiang on 19-5-4.
//
#include "myslam/viewer.h"
#include "myslam/feature.h"
#include "myslam/frame.h"

#include <pangolin/pangolin.h>
#include <opencv2/opencv.hpp>

using namespace cv;
namespace myslam {


#define USE_VIEWER 0

Viewer::Viewer() {
    if(USE_VIEWER==1)
        viewer_thread_ = std::thread(std::bind(&Viewer::ThreadLoop, this));
}

void Viewer::Close() {
    viewer_running_ = false;
    if(USE_VIEWER==1)
        viewer_thread_.join();
}

void Viewer::AddCurrentFrame(Frame::Ptr current_frame) {
    std::unique_lock<std::mutex> lck(viewer_data_mutex_);
    current_frame_ = current_frame;
}

void Viewer::UpdateMap() {
        std::unique_lock<std::mutex> lck(viewer_data_mutex_);
        assert(map_ != nullptr);
        // active_keyframes_ = map_->GetActiveKeyFrames();
        // active_landmarks_ = map_->GetActiveMapPoints();
            active_keyframes_ = map_->GetAllKeyFrames();
        active_landmarks_ = map_->GetAllMapPoints();
        map_updated_ = true;
}


void Viewer::ThreadLoop() {
    if(USE_VIEWER==1){
        pangolin::CreateWindowAndBind("MySLAM", 1024, 768);         //创建一个窗口  
        glEnable(GL_DEPTH_TEST);    //启动深度测试,该功能会使得pangolin只会绘制朝向镜头的那一面像素点，
                                                        //避免容易混淆的透视关系出现，因此在任何3D可视化中都应该开启该功能。
        glEnable(GL_BLEND); 
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

        // 定义相机投影模型：ProjectionMatrix(w, h, fu, fv, u0, v0, zNear, zFar)
        // 定义观测方位向量：观测点位置：(mViewpointX mViewpointY mViewpointZ)
        //                观测目标位置：(0, 0, 0)
        //                观测的方位向量：(0.0,-1.0, 0.0)
        pangolin::OpenGlRenderState vis_camera(         //定义投影和初始模型
            pangolin::ProjectionMatrix(1024, 768, 400, 400, 512, 384, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -5, -10, 0, 0, 0, 0.0, -1.0, 0.0));

        // Add named OpenGL viewport to window and provide 3D Handler

        // 定义地图面板
        // SetBound最后一个参数（-1024.0f/768.0f）为显示长宽比
        pangolin::View& vis_display =
            pangolin::CreateDisplay()
                .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
                .SetHandler(new pangolin::Handler3D(vis_camera));

        
        const float green[3] = {0, 1, 0};

        while (!pangolin::ShouldQuit() && viewer_running_) {
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //清除屏幕
            glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
            vis_display.Activate(vis_camera);   //使该视口生效，并进行渲染

            std::unique_lock<std::mutex> lock(viewer_data_mutex_);
            if (current_frame_) {
                DrawFrame(current_frame_, green);
                FollowCurrentFrame(vis_camera);

                cv::Mat img = PlotFrameImage();
                cv::imshow("image", img);
                cv::waitKey(1);
            }

            if (map_) {
                DrawMapPoints();
            }

            pangolin::FinishFrame();
            usleep(5000);   //线程挂起5ms
        }
        // cv::waitKey(0);
        LOG(INFO) << "Stop viewer";
    }
    else{
        while(viewer_running_){
            usleep(1);
        }
    }

}

cv::Mat Viewer::PlotFrameImage() {
    cv::Mat img_out;
    cv::cvtColor(current_frame_->left_img_, img_out, COLOR_GRAY2BGR);
    for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) {
        if (current_frame_->features_left_[i]->map_point_.lock()) {
            auto feat = current_frame_->features_left_[i];
            cv::circle(img_out, feat->position_.pt, 2, cv::Scalar(0, 250, 0),
                       2);
        }
    }
    return img_out;
}

void Viewer::FollowCurrentFrame(pangolin::OpenGlRenderState& vis_camera) {
    SE3 Twc = current_frame_->Pose().inverse();
    pangolin::OpenGlMatrix m(Twc.matrix());
    vis_camera.Follow(m, true);
}

void Viewer::DrawFrame(Frame::Ptr frame, const float* color) {
    SE3 Twc = frame->Pose().inverse();
    const float sz = 0.1;
    const int line_width = 2.0;
    const float fx = 718;
    const float fy = 718;
    const float cx = 640;
    const float cy = 512;
    const float width = 1280;
    const float height = 1024;

    glPushMatrix();

    Sophus::Matrix4f m = Twc.matrix().template cast<float>();
    glMultMatrixf((GLfloat*)m.data());
 
    if (color == nullptr) {
        glColor3f(1, 0, 0);
    } else
        glColor3f(color[0], color[1], color[2]);

    glLineWidth(line_width);
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
    glVertex3f(0, 0, 0);
    glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
    glVertex3f(0, 0, 0);
    glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
    glVertex3f(0, 0, 0);
    glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);

    glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
    glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);

    glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
    glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);

    glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
    glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);

    glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
    glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);

    glEnd();
    glPopMatrix();
}

void Viewer::DrawMapPoints() {
    const float blue[3] = {0, 0, 1};
    const float red[3] = {1.0, 0, 0};
    for (auto& kf : active_keyframes_) {
        DrawFrame(kf.second, red);
    }


//glColor3f(0.0, 0.0, 0.0);  --> 黑色  
// glColor3f(1.0, 0.0, 0.0);  --> 红色  
// glColor3f(0.0, 1.0, 0.0);  --> 绿色  
// glColor3f(0.0, 0.0, 1.0);  --> 蓝色  
// glColor3f(1.0, 1.0, 0.0);  --> 黄色  
// glColor3f(1.0, 0.0, 1.0);  --> 品红色  
// glColor3f(0.0, 1.0, 1.0);  --> 青色  
// glColor3f(1.0, 1.0, 1.0);  --> 白色


    glPointSize(1);
    glBegin(GL_POINTS);
    for (auto& landmark : active_landmarks_) {
        auto pos = landmark.second->Pos();
        glColor3f(blue[0], blue[1], blue[2]);
        glVertex3d(pos[0], pos[1], pos[2]);
    }
    glEnd();
}

}  // namespace myslam
